﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;
using Microsoft.Kinect.Toolkit;

namespace PupillaryDistance.DesktopUI
{
    public partial class MainWindow : Window
    {
        private static readonly int Bgr32BytesPerPixel = (PixelFormats.Bgr32.BitsPerPixel + 7) / 8;
        private readonly KinectSensorChooser sensorChooser = new KinectSensorChooser();
        private WriteableBitmap colorImageWritableBitmap;
        private WriteableBitmap depthImageWritableBitmap;
        private byte[] _bitmapBits;
        private byte[] colorImageData;
        private short[] depthImageData = new short[0];
        private ColorImagePoint[] _mappedDepthLocations;
        private ColorImageFormat currentColorImageFormat = ColorImageFormat.Undefined;
        private bool ShowDepthTracker { get; set; }

        public MainWindow()
        {
            InitializeComponent();

            var faceTrackingViewerBinding = new Binding("Kinect") { Source = sensorChooser };
            faceTrackingViewer.SetBinding(FaceTrackingViewer.KinectProperty, faceTrackingViewerBinding);

            sensorChooser.KinectChanged += SensorChooserOnKinectChanged;
            sensorChooser.Start();
        }

        private void SensorChooserOnKinectChanged(object sender, KinectChangedEventArgs kinectChangedEventArgs)
        {
            KinectSensor oldSensor = kinectChangedEventArgs.OldSensor;
            KinectSensor newSensor = kinectChangedEventArgs.NewSensor;

            if (oldSensor != null)
            {
                oldSensor.AllFramesReady -= KinectSensorOnAllFramesReady;
                oldSensor.ColorStream.Disable();
                oldSensor.DepthStream.Disable();
                oldSensor.DepthStream.Range = DepthRange.Default;
                oldSensor.SkeletonStream.Disable();
                oldSensor.SkeletonStream.EnableTrackingInNearRange = false;
                oldSensor.SkeletonStream.TrackingMode = SkeletonTrackingMode.Default;
            }

            if (newSensor != null)
            {
                try
                {
                    newSensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
                    newSensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);

                    try
                    {
                        // This will throw on non Kinect For Windows devices.
                        newSensor.DepthStream.Range = DepthRange.Near;
                        newSensor.SkeletonStream.EnableTrackingInNearRange = true;
                    }
                    catch (InvalidOperationException)
                    {
                        newSensor.DepthStream.Range = DepthRange.Default;
                        newSensor.SkeletonStream.EnableTrackingInNearRange = false;
                    }

                    newSensor.SkeletonStream.TrackingMode = SkeletonTrackingMode.Seated;
                    newSensor.SkeletonStream.Enable();
                    newSensor.AllFramesReady += KinectSensorOnAllFramesReady;
                }
                catch (InvalidOperationException)
                {
                    // This exception can be thrown when we are trying to
                    // enable streams on a device that has gone away.  This
                    // can occur, say, in app shutdown scenarios when the sensor
                    // goes away between the time it changed status and the
                    // time we get the sensor changed notification.
                    //
                    // Behavior here is to just eat the exception and assume
                    // another notification will come along if a sensor
                    // comes back.
                }
            }
        }

        private void WindowClosed(object sender, EventArgs e)
        {
            sensorChooser.Stop();
            faceTrackingViewer.Dispose();
        }

        private void KinectSensorOnAllFramesReady(object sender, AllFramesReadyEventArgs allFramesReadyEventArgs)
        {
            using (var colorImageFrame = allFramesReadyEventArgs.OpenColorImageFrame())
            {
                var depth = allFramesReadyEventArgs.OpenDepthImageFrame();

                if (colorImageFrame == null)
                {
                    return;
                }

                // Make a copy of the color frame for displaying.
                var haveNewFormat = this.currentColorImageFormat != colorImageFrame.Format;
                if (haveNewFormat)
                {
                    this.currentColorImageFormat = colorImageFrame.Format;
                    this.colorImageData = new byte[colorImageFrame.PixelDataLength];
                    this.colorImageWritableBitmap = new WriteableBitmap(
                        colorImageFrame.Width, colorImageFrame.Height, 96, 96, PixelFormats.Bgr32, null);
                    ColorImage.Source = this.colorImageWritableBitmap;
                }

                _bitmapBits = new byte[colorImageFrame.Height * colorImageFrame.Width * 4];
                colorImageFrame.CopyPixelDataTo(this.colorImageData);
                this.colorImageWritableBitmap.WritePixels(
                    new Int32Rect(0, 0, colorImageFrame.Width, colorImageFrame.Height),
                    this.colorImageData, colorImageFrame.Width * Bgr32BytesPerPixel, 0);
            }

            // Drawing Depth 
            if (ShowDepthTracker == true)
            {
                using (DepthImageFrame depthFrame = allFramesReadyEventArgs.OpenDepthImageFrame())
                {
                    if (depthFrame != null)
                    {
                        if (depthImageData.Length != depthFrame.PixelDataLength)
                        {
                            depthImageData = new short[depthFrame.PixelDataLength];
                            _mappedDepthLocations = new ColorImagePoint[depthFrame.PixelDataLength];
                        }

                        depthFrame.CopyPixelDataTo(depthImageData);

                        for (int i = 0; i < colorImageData.Length; i += 4)
                        {
                            _bitmapBits[i + 3] = 255;
                            _bitmapBits[i + 2] = colorImageData[i + 2];
                            _bitmapBits[i + 1] = colorImageData[i + 1];
                            _bitmapBits[i] = colorImageData[i];
                        }

                        KinectSensor sensor = KinectSensor.KinectSensors[0];
                        DepthImagePixel[] dp = new DepthImagePixel[depthImageData.Length];
                        depthFrame.CopyDepthImagePixelDataTo(dp);

                        sensor.CoordinateMapper.MapDepthFrameToColorFrame(
                            DepthImageFormat.Resolution640x480Fps30, dp,
                            ColorImageFormat.RgbResolution640x480Fps30, _mappedDepthLocations);

                        for (int i = 0; i < depthImageData.Length; i++)
                        {
                            int depthVal = depthImageData[i] >> DepthImageFrame.PlayerIndexBitmaskWidth;

                            // Put in the overlay of depth values < 1 meters.       
                            if ((depthVal < 1000) && (depthVal > 400))
                            {
                                ColorImagePoint point = _mappedDepthLocations[i];

                                if ((point.X >= 0 && point.X < 500) && (point.Y >= 0 && point.Y < 480))
                                {
                                    int baseIndex = (point.Y * 640 + point.X) * 4;
                                    _bitmapBits[baseIndex] = (byte)((_bitmapBits[baseIndex] + 255) >> 1);
                                    _bitmapBits[baseIndex + 1] = (byte)((_bitmapBits[baseIndex + 1] + 255) >> 1);
                                    _bitmapBits[baseIndex + 2] = (byte)((_bitmapBits[baseIndex] + 255) >> 1);
                                }

                            }
                        }

                        this.depthImageWritableBitmap = new WriteableBitmap(
                            depthFrame.Width, depthFrame.Height, 96, 96, PixelFormats.Bgr32, null);
                        depthImageWritableBitmap.WritePixels(new Int32Rect(
                            0, 0, colorImageWritableBitmap.PixelWidth, colorImageWritableBitmap.PixelHeight), _bitmapBits, colorImageWritableBitmap.PixelWidth * sizeof(int), 0);
                        DepthImage.Source = depthImageWritableBitmap;
                    }
                }
            }
            else
            {
                DepthImage.Source = null;
            }

            if (faceTrackingViewer.IsBeingTracked == true)
            {
                lblDetectingPupils.Foreground = Brushes.DarkRed;
                lblDetectingBody.Foreground = Brushes.Gray;
            }
            else
            {
                lblDetectingPupils.Foreground = Brushes.Gray;
                lblDetectingBody.Foreground = Brushes.DarkRed;
            }
        }

        private void cbMethod1_Click(object sender, RoutedEventArgs e)
        {
            faceTrackingViewer.MethodOneEnabled = (bool)((CheckBox)sender).IsChecked;
        }

        private void cbMethod2_Click(object sender, RoutedEventArgs e)
        {
            faceTrackingViewer.MethodTwoEnabled = (bool)((CheckBox)sender).IsChecked;
        }

        private void cbMethod3_Click(object sender, RoutedEventArgs e)
        {
            faceTrackingViewer.MethodThreeEnabled = (bool)((CheckBox)sender).IsChecked;
        }

        private void cbMethod4_Click(object sender, RoutedEventArgs e)
        {
            faceTrackingViewer.MethodFourEnabled = (bool)((CheckBox)sender).IsChecked;
        }

        private void cbMethod5_Click(object sender, RoutedEventArgs e)
        {
            faceTrackingViewer.MethodFiveEnabled = (bool)((CheckBox)sender).IsChecked;
        }

        private void cbDepthTracker_Click(object sender, RoutedEventArgs e)
        {
            ShowDepthTracker = (bool)((CheckBox)sender).IsChecked;
        }

        private void cbEyeTracker_Click(object sender, RoutedEventArgs e)
        {
            faceTrackingViewer.EyeTracker = (bool)((CheckBox)sender).IsChecked;
        }
    }
}
